﻿using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using W3C.Soap;
using System.IO.Ports;
using System.Threading;
using System.Text;
using System.Linq;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using Mechatronics.Robotics.Transports;
using Mechatronics.Robotics.Protocols;

namespace Mechatronics.Robotics.Silver.Brick
{
    [DisplayName("Silver Robot Service")]
    [Description("The service that provide access to Silver Robot")]
    [Contract(Contract.Identifier)]
    public class SilverBrickService : DsspServiceBase
    {
        #region Service definition

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        [InitialStatePartner(Optional = false)]
        private SilverBrickState _state = new SilverBrickState();

        [ServicePort("/silverbrick", AllowMultipleInstances = false)]
        private SilverBrickOperations _mainPort = new SilverBrickOperations();

        public SilverBrickService(DsspServiceCreationPort creationPort) :
            base(creationPort)
        {
        }

        #endregion

        // this is a internal port used to sending 'connect to robot' messages
        // by this service to itself repeatidly
        private Port<int> _internalPort = new Port<int>();

        // this stores information about time beetween two connections to robot in miliseconds
        private const int ConnectionFrequency = 100;

        private const int ConnectionBaudRate = 38400;

        private ITransport connection;

        // represents Silver robot protocol
        private readonly SilverProtocol protocol = new SilverProtocol();

        protected override void Start()
        {
            // initialize connection
            connection = new TransportRS232(_state.PortName, ConnectionBaudRate);
            try
            {
                connection.Connect();
            }
            catch (Exception ex)
            {
                LogError(ex);
                return;
            }

            connection.DataReceived += DataReceived;

            Activate(
                Arbiter.Receive(true,
                _internalPort,
                GetDataFromRobot));
            _internalPort.Post(0);

            base.Start();
        }

        private List<byte> buffer = new List<byte>();

        private void DataReceived(byte[] data)
        {
            // collect data until command is full

            var q =
                from b in data
                select b;

            buffer.AddRange(q);

            int pos = buffer.IndexOf((byte)'\n');

            if (pos < 0)
                return;

            byte[] rsp = buffer.GetRange(0, pos + 1).ToArray<byte>();
            buffer.RemoveRange(0, pos + 1);

            byte[] response;
            try
            {
                response = protocol.ParseResponse(rsp);
            }
            catch (Exception ex)
            {
                LogError(ex);
                return;
            }

            // check for possible responses
            try
            {
                int ping;
                double sharp;
                protocol.ParseReadSonarResponse(response, out ping, out sharp);
                SendSonarNotification(ping);
                //SendSharpNotification(7, sharp);
            }
            catch { }

            try
            {
                double battery = protocol.ParseReadBatteryResponse(response);
                SendBatteryNotification(battery);
            }
            catch { }

            try
            {
                int[] sharps = protocol.ParseReadIRResponse(response);
                SendIRNotification(sharps);
            }
            catch { }
        }

        private void SendIRNotification(int[] sharps)
        {
            //DateTime now = DateTime.Now;
            //array.AnalogSensorArrayState s = new array.AnalogSensorArrayState();
            //s.Sensors = new List<Microsoft.Robotics.Services.AnalogSensor.Proxy.AnalogSensorState>();
            //for (int i = 0; i < sharps.Length; ++i)
            //{
            //    double msr = sharps[i] * 5.0 / 255.0;
            //    s.Sensors.Add(new Microsoft.Robotics.Services.AnalogSensor.Proxy.AnalogSensorState()
            //    {
            //        HardwareIdentifier = i,
            //        // TODO:
            //        Pose = new Pose(),
            //        TimeStamp = now,
            //        RawMeasurement = msr,
            //        RawMeasurementRange = 5,
            //        NormalizedMeasurement = msr / 5.0,
            //    });
            //}

            //base.SendNotification(_submgrPort, new IRUpdated(s));
        }

        private void SendBatteryNotification(double measure)
        {
            //battery.BatteryState bs = new battery.BatteryState()
            //{
            //    MaxBatteryPower = 12.0,
            //    PercentBatteryPower = measure / 12.0,
            //    PercentCriticalBattery = 0.05,
            //};

            //base.SendNotification(_submgrPort, new BatteryUpdated(bs));
        }

        private void SendSonarNotification(int measure)
        {
            //sonar.SonarState ss = new sonar.SonarState()
            //{
            //    // TODO: get this data right!
            //    AngularRange = 30,
            //    AngularResolution = 1,
            //    DistanceMeasurement = measure * 10.0,
            //    DistanceMeasurements = new double[] { measure * 10.0 },
            //    HardwareIdentifier = 1,
            //    MaxDistance = 3000,
            //    Pose = new Pose(),
            //    TimeStamp = DateTime.Now
            //};

            //base.SendNotification(_submgrPort, new SonarUpdated(ss));
        }

        private int commCount = 0;

        void GetDataFromRobot(int v)
        {
            SendSpeedCommand();

            if (commCount > 5)
            {
                GetSonar();

                GetIR();

                GetBatteryStatus();

                commCount = 0;
            }

            commCount++;

            // run itself after timeout
            Activate(
                Arbiter.Receive(false,
                TimeoutPort(ConnectionFrequency),
                z => _internalPort.Post(0)));
        }

        private void GetIR()
        {
            byte[] packet = protocol.ReadIRRequest();
            connection.Send(packet);
        }

        private void GetSonar()
        {
            byte[] packet = protocol.ReadSonarRequest();
            connection.Send(packet);
        }

        private void GetBatteryStatus()
        {
            byte[] packet = protocol.ReadBatteryRequest();
            connection.Send(packet);
        }

        private void SendSpeedCommand()
        {
            //byte[] packet = protocol.SetSpeedRequest(_state.MotorSpeedLeft, _state.MotorSpeedRight);
            //connection.Send(packet);
        }

        #region Service handlers

        //[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        //public void SetMotorSpeedHandler(SetMotorSpeed set_motor_speed)
        //{
        //    if (set_motor_speed.Body.Motor == Motors.Left)
        //    {
        //        _state.MotorSpeedLeft = set_motor_speed.Body.Speed;
        //        set_motor_speed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        //    }
        //    else if (set_motor_speed.Body.Motor == Motors.Right)
        //    {
        //        _state.MotorSpeedRight = set_motor_speed.Body.Speed;
        //        set_motor_speed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        //    }
        //    else
        //        set_motor_speed.ResponsePort.Post(Fault.FromException(
        //            new Exception("Invalid motor selected")));
        //}

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public void GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public void SubscribeHandler(Subscribe subscribe)
        {
            SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
        }

        #endregion
    }
}
